/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/GeometrySrc/angle.cpp
 * @Description  : 实现了角度类，提供了角度的各种操作，包括角度的创建、转换、规范化、比较和运算等功能。
 *                 支持弧度和度数两种模式，并且可以处理角度之间的差异和范围检测。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:49:54
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include <math.h>
#include "geometry.h"
float Angle::resolution_ = PI / 180;     // Default resolution is 1 degree.

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CAngle".
     
//
//   CAngle: The constructor.
//
Angle::Angle(float value, AngleMode mode) 
{
    radian_ = (mode == AngleMode::InRadian) ? value : ToRadian(value);
    radian_ = NormAngle(radian_);
}

//
//   Construct a corner from two points.
//
Angle::Angle(const Point& pt1, const Point& pt2) 
{
    radian_ = std::atan2(pt2.y_ - pt1.y_, pt2.x_ - pt1.x_);
    radian_ = NormAngle(radian_);
}

//
//    Degree: Get the angle value in "degree".
//
float Angle::Degree() const 
{
    return ToDegree(radian_);
}

//
//    Quadrant: Return the angle's Quadrant. (1/2/3/4).
//    PI is 3
int Angle::Quadrant() const 
{
    if (radian_ < PI / 2) return 1;
    if (radian_ < PI) return 2;
    if (radian_ < 3 * PI / 2) return 3;
    return 4;
}

//
//   ToRadian: Convert a degree angle into a radian value.
//
float Angle::ToRadian(float degree) 
{
    return (PI / 180) * degree;
}

//
//   ToDegree: Convert a radian angle into a degree value.
//
float Angle::ToDegree(float radian) 
{
    return (180 / PI) * radian;
}

//
//    NormAngle: Normalize an radian angle into the range [0, 2*PI).
//
float Angle::NormAngle(float radian) 
{
    if (radian > 10000.0f || radian < -10000.0f) {
		radian = PI / 3;
	}

    while (radian < 0) {
        radian += 2 * PI;
    }

    while (radian >= 2 * PI) {
        radian -= 2 * PI;
    }
    return radian;
}

//
//    NormAngle: Normalize an radian angle into the range [-PI, PI).
//
float Angle::NormAngle2(float radian) 
{
    while (radian < -PI) {
        radian += 2 * PI;
    }

    while (radian >= PI) {
        radian -= 2 * PI;
    }
    return radian;
}

/**
    * Returns an equivalent angle to the first parameter such that the difference
    * when the first parameter is subtracted from this new value is an angle
    * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi.
    * @param subtrahend
    * @param minuend
    * @return normalized subtrahend angle
    */
double Angle::NormalizeAngleDifference(double subtrahend, double minuend) 
{
    while ((minuend - subtrahend) >= PI){
        if((minuend - subtrahend) >= _2_PI){
            subtrahend += (unsigned int)((minuend - subtrahend) / _2_PI) * _2_PI;
        }
        else {
            subtrahend += _2_PI;
        }
    }
    while((minuend - subtrahend) < -PI){
        if((minuend - subtrahend) < -_2_PI){
            subtrahend -= (unsigned int)((minuend - subtrahend) / -_2_PI) * _2_PI;
        }
        else {
            subtrahend -= _2_PI;
        }
    }
    return subtrahend;
}

//
//    NormAngle: Normalize the angle into the range [-PI, PI).
//
float Angle::NormAngle2()
{
    // Scale the angle into [-PI, +)
    while(radian_ < -PI)
        radian_ += 2*PI;

    // Scale the angle into [-PI, PI)
    while (radian_ >= PI)
        radian_ -= 2*PI;

    return radian_;
}

//
//   SetAngleReso: Set the resolution (in radian) of angle comparison.
//
float Angle::SetReso(float resolution) 
{
    float old_resolution = resolution_;
    resolution_ = resolution;
    return old_resolution;
}

// 
//   Rotate: Rotate the angle by a given value.
//
void Angle::Rotate(float angle) 
{
    radian_ = NormAngle(radian_ + angle);
}

//
//   Operator "-": Return the negation of the angle.
//
Angle Angle::operator-() const 
{
    return Angle(-radian_);
}

//
//   Operator "!": Return the reverse-directioned angle.
//
Angle Angle::operator!() const 
{
    return Angle(radian_ + PI);
}

//
//   Operator "+": Return the sum of 2 angles.
//
Angle Angle::operator+(const Angle& ang) const 
{
    return Angle(radian_ + ang.radian_);
}

//
//   Operator "-": Return the difference of 2 angles.
//
Angle Angle::operator-(const Angle& ang) const 
{
    return Angle(radian_ - ang.radian_);
}

//
//   Operator "+=": Increment of angle.
//
void Angle::operator+=(const Angle& ang) 
{
    radian_ = NormAngle(radian_ + ang.radian_);
}


//
//   Operator "-=": Decrement of angle.
//
void Angle::operator-=(const Angle& ang) 
{
    radian_ = NormAngle(radian_ - ang.radian_);
}

//
//   Operator "==": Test if the 2 given angles are equal.
//
//   NOTE: 
//      If the difference of the 2 angles is less than the "resolution",
//   the 2 angles will be regarded as equal.
//
bool Angle::operator==(const Angle& ang) const 
{
    float diff = std::fabs(radian_ - ang.radian_);
    return (diff < resolution_ || 2 * PI - diff < resolution_);
}

//
//   Operator "!=": Test if the 2 given angles are not equal.
//    
bool Angle::operator!=(const Angle& ang) const 
{
    return !(*this == ang);
}

//
//   Operator "ApproxEqualTo": Test if the 2 given angles are equal.
//
bool Angle::ApproxEqualTo(const Angle& ang, float maxdiffrad) const
{
    if (maxdiffrad == 0)
        maxdiffrad = resolution_;

    float temp = (float)fabs(radian_ - ang.radian_);
    return (temp < maxdiffrad || 2*PI - temp < maxdiffrad);
}

//
//   Operator ">": Test if the first angle is bigger than the second one.
//
bool Angle::operator>(const Angle& ang) const 
{
    return ((radian_ > ang.radian_) && (*this != ang));
}

//
//    Operator "<": Test if the first angle is smaller than the second one.
//
bool Angle::operator<(const Angle& ang) const 
{
    return ((radian_ < ang.radian_) && (*this != ang));
}

//
//   Operator ">=": Test if the first angle is bigger than or equal to
//   the second one.
//
bool Angle::operator>=(const Angle& ang) const 
{
    return ((radian_ > ang.radian_) || (*this == ang));
}

//
//   Operator "<=": Test if the first angle is smaller than or equal to
//   the second one.
//
bool Angle::operator<=(const Angle& ang) const 
{
    return ((radian_ < ang.radian_) || (*this == ang));
}

//
//   Operator "=": Set the angle value.
//
void Angle::operator=(float rad) 
{
    radian_ = NormAngle(rad);
}

//
//   Operator "+": Return the sum of 2 angles.
//
Angle Angle::operator+(float rad) const 
{
    return Angle(radian_ + rad);
}


//
//   Operator "-": Return the difference of 2 angles.
//
Angle Angle::operator-(float rad) const 
{
    return Angle(radian_ - rad);
}

//
//   Operator "+=": Increment of angle.
//
void Angle::operator+=(float rad) 
{
    radian_ = NormAngle(radian_ + rad);
}

//
//   Operator "-=": Decrement of angle.
//
void Angle::operator-=(float rad) 
{
    radian_ = NormAngle(radian_ - rad);
}

//
//   Operator "==": Check whether the angle is equal to the specified radian value.
//
bool Angle::operator==(float rad) const 
{
    float diff = std::fabs(radian_ - NormAngle(rad));
    return (diff < resolution_ || 2 * M_PI - diff < resolution_);
}

bool Angle::operator!=(float rad) const 
{
    return !(*this == rad);
}

bool Angle::operator>(float rad) const 
{
    rad = NormAngle(rad);
    return ((radian_ > rad) && (*this != rad));
}

bool Angle::operator<(float rad) const 
{
    rad = NormAngle(rad);
    return ((radian_ < rad) && (*this != rad));
}

bool Angle::operator>=(float rad) const 
{
    rad = NormAngle(rad);
    return ((radian_ > rad) || (*this == rad));
}

bool Angle::operator<=(float rad) const 
{
    rad = NormAngle(rad);
    return ((radian_ < rad) || (*this == rad));
}

//
//  Calculates the difference between the current Angle object and another Angle object
//
float Angle::GetDifference(const Angle& another) const 
{
    float ang1 = NormAngle(radian_);
    float ang2 = NormAngle(another.radian_);
    if (ang1 > ang2) {
        return (ang1 - ang2 > PI) ? 2 * PI + ang2 - ang1 : ang1 - ang2;
    } else if (ang1 < ang2) {
        return (ang2 - ang1 > PI) ? 2 * PI + ang1 - ang2 : ang2 - ang1;
    } else {
        return 0;
    }
}

//
//  Determines whether the current Angle is between two given angles
//
bool Angle::InRange(const Angle& ang1, const Angle& ang2) const 
{
    Angle angDiff2 = ang2 - ang1;
    Angle angDiff1 = *this - ang1;
    return angDiff2.radian_ > angDiff1.radian_;
}

//////////////////////////////////////////////////////////////////////////////
//   The followings are some overloaded functions: sin(), cos(), tan()
//
//   sin: Overloaded function of sin().
//
float sin(const Angle& ang) 
{
    return std::sin(ang.radian_);
}

//
//   cos: Overloaded function of cos().
//
float cos(const Angle& ang) 
{
    return std::cos(ang.radian_);
}

//
//   tan: Overloaded function of tan().
//
float tan(const Angle& ang) 
{
    return std::tan(ang.radian_);
}

//
//   Get the absolute value of an angle. (the absolute value of -30 degree is 30 degree)
//
Angle abs(const Angle& ang) 
{
    Angle angle = ang;
    if (angle.radian_ > PI) {
        angle.radian_ = 2 * PI - angle.radian_;
    }
    return angle;
}

//
//   Calculate the difference between the two angles, 
//    making sure the difference is between 0 and PI
//
float AngleDiff(float angle1, float angle2)
{
	if (angle1 > 10000.0 || angle1 < -10000.0)
	{
		angle1 = PI/3;
	}

	if (angle2 > 10000.0 || angle2 < -10000.0)
	{
		angle2 = PI/3;
	}


	while(angle1 < 0) angle1 += 2*PI;
	while(angle1 > 2*PI) angle1 -= 2*PI;
	while(angle2 < 0) angle2 += 2*PI;
	while(angle2 > 2*PI) angle2 -= 2*PI;
	float diff = (float)fabs(angle1 - angle2);
	if(diff <= PI)
		return diff;
	else
		return 2*PI - diff;
}

/// @brief Normalizes the given radian value into the range [-PI, PI]
/// @param radian 
/// @return 
float NormAngle2(float radian)
{
    // Scale the angle into [-PI, +)
    while(radian < -PI)
        radian += 2*PI;

    // Scale the angle into [-PI, PI)
    while (radian >= PI)
        radian -= 2*PI;

    return radian;
}

bool LessDegree(const Angle& src_angle, float degree) 
{
    return src_angle <= Angle(degree, AngleMode::InDegree);
}

//
//    NormAngle: Normalize the angle into the range [0, 2 * PI).
//
float Angle::NormAngle()
{
    float rad = radian_;

    // Scale the angle into [0, +)
    while(rad < 0)
        rad += 2*PI;

    // Scale the angle into [0, 2 * PI)
    while (rad >= 2 * PI)
        rad -= 2*PI;

    return rad;
}
